/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.robotOutput;

import com.team1160.breakaway.ballControl.Dribbler;
import com.team1160.breakaway.ballControl.Kicker;
import com.team1160.breakaway.camera.Cam_Drive;
import com.team1160.breakaway.drive.Drive;
import com.team1160.breakaway.endGame.Arm;
import com.team1160.breakaway.model.robotCommand.RobotCommand;

/**
 *
 * @author CJ
 */
public class RobotOutput {
    //TODO Everyone

    protected Kicker kicker;
    protected Dribbler dribbler;
    protected Drive driveTrain;
    protected Cam_Drive camDrive;
    protected Arm arm;

    public RobotOutput(Kicker kicker,
            Dribbler dribbler,
            Drive driveTrain,
            Cam_Drive camDrive,
            Arm arm){
        this.camDrive = camDrive;
        this.kicker = kicker;
        this.dribbler = dribbler;
        this.driveTrain = driveTrain;
        this.arm = arm;
    }

    public void implementActions(RobotCommand robotCommand){
        articulateKicker(robotCommand);
        articulateDriveTrain(robotCommand);
        //articulateDribbler(robotCommand);
        articulateArm(robotCommand);
    }

    protected void articulateArm(RobotCommand robotCommand){
        this.arm.activateClaw(robotCommand.getToggleClaw());
        this.arm.activateMotor(robotCommand.getActivateWinch());
    }

    protected void articulateDribbler(RobotCommand robotCommand){
        if(robotCommand.dribblerOn()){
            this.dribbler.start(robotCommand.dribblerSpeed());
        }else{
            this.dribbler.stop();
        }
    }

    protected void articulateKicker(RobotCommand robotCommand){
        if(robotCommand.extendShooter())
            this.kicker.extendFire();
        else
            this.kicker.retractFire();


        if(robotCommand.holdShooter())
            this.kicker.retractLatch();
        else
            this.kicker.extendLatch();
    }

    protected void articulateDriveTrain(RobotCommand robotCommand){
        if(robotCommand.motorStop()){
            byte[] motorsStopped = robotCommand.motorsStopped();
            for(int i = 0; i < motorsStopped.length; ++i){
                if(motorsStopped[i] != -1){
                    this.driveTrain.motorStop(motorsStopped[i]);
                }
            }
        }else{
            this.driveTrain.motorStop((byte)10);
        }
        this.driveTrain.setLeftRightMotorSpeeds(robotCommand.getDriveLeft(), robotCommand.getDriveRight());
    }
}
